import msvcrt
from nokov.nokovsdk import *
import time
import sys, getopt
import torch
from collections import deque
from position import *
from utils import *
from matplotlib import pyplot as plt
import matplotlib
import numpy as np
import copy

done=False
tdq=deque()
pdq=deque()
p=poses()
v=speeds()
frame_count=0
falt=False

vl=np.zeros((1,7,2))

def get_ball_pose(frameData, ball=True):
    n=frameData.nOtherMarkers
    markers=torch.zeros([n, 3])
    if n!=0:
        markers[:,:]=torch.tensor([[frameData.OtherMarkers[j][k]/1000 for k in range(3)]for j in range(n)])
        # if isinstance(posi, int): # if want a ball position, but get 0 or multiple unknown markers
        #     return None
        if ball:
            if markers.shape[0]==1:# 只有一个未知点，即足球
                return pose(markers[0][0],markers[0][1],0) # (x,y,0)
            elif markers.shape[0]==2 and abs(torch.max(markers[0]-markers[1]))<0.06:
                t=torch.mean(markers,dim=0)
                return pose(t[0],t[1],0)
            elif markers.shape[0]==3 and abs(torch.max(torch.cat((markers[0]-markers[1],markers[2]-markers[1],markers[0]-markers[2]))))<0.06: 
                t=torch.mean(markers,dim=0)   
                return pose(t[0],t[1],0)
            else:
                # print('*'*10)
                # print(markers)
                return None
        else:
            return torch.Tensor(markers[:,:]) # 返回所有未知点坐标

def get_device_pose(frameData, i):
    body = frameData.RigidBodies[i]
    return pose(body.x/1000,body.y/1000,get_ori(body.qz,body.qw))

def py_data_func(pFrameOfMocapData, pUserData):
    global p,v,frame_count, tdq, pdq,falt,vl
    # if pFrameOfMocapData == None:  
    #     print("Not get the data frame.\n")
    #     return None
    # else:
    frameData = pFrameOfMocapData.contents
    pb=get_ball_pose(frameData)
    # print(pb)
    if pb is None:
        pb=pose(1,0)
    p[0]=pb
    for i in range(6):
        p[i+1]=get_device_pose(frameData, i) # 在xingying中依次勾选EP1~6

    if frame_count%5==0:
        tdq.append(time.time())
        pdq.append(copy.deepcopy(p))
        if len(tdq)>2 and len(pdq)>2:
            tdq.popleft()
            pdq.popleft()
            dt=tdq[1]-tdq[0] # 单位：s
            # print(f'dt:{dt}')
            # dt=0.0833
            # if dt>0.2 or dt<0.14:
            #     print(dt)
            if dt!=0:
                for i in range(0,7):
                    vt=speed((pdq[1][i].x-pdq[0][i].x)/dt,(pdq[1][i].y-pdq[0][i].y)/dt)
                    if vt.get_norm()<3:
                        v[i]=vt
                    else:
                        logging.debug(f'v{i}异常：{vt}')

                # print(v.get_arr())
                # print(np.expand_dims(v.get_arr(),0))
                # print(vl)
                # print(f'v:{v[1]}')
                vl=np.concatenate((vl,np.expand_dims(v.get_arr(),0)),0)
                # if vl.shape[0]>100:
                #     vl=np.delete(vl,0,axis=0)
    frame_count+=1
    # check_catched(p.p2, p.pb)

def py_msg_func(iLogLevel, szLogMessage):
    szLevel = "None"
    if iLogLevel == 4:
        szLevel = "Debug"
    elif iLogLevel == 3:
        szLevel = "Info"
    elif iLogLevel == 2:
        szLevel = "Warning"
    elif iLogLevel == 1:
        szLevel = "Error"
  
    print("[%s] %s" % (szLevel, cast(szLogMessage, c_char_p).value))


def run(event, gp,gv):
    global done,p,v

    serverIp = '10.1.1.198'
    
    print ('serverIp is %s' % serverIp)
    client = PySDKClient()
    client.PySetVerbosityLevel(0)
    client.PySetMessageCallback(py_msg_func)
    client.PySetDataCallback(py_data_func, None)
    ret = client.Initialize(bytes(serverIp, encoding = "utf8"))

    if ret == 0:
        print("Connect to the Seeker Succeed")
    else:
        print("Connect Failed: [%d]" % ret)
        exit(0)

    print("=============positioning")
    event.set()
    
    # matplotlib.use('agg')
    # plt.ion()
    # plt.figure(figsize=(9,9))
    # plt.yticks(np.arange(-3,3,0.2))

    while(not done):
        # if (msvcrt.kbhit() and msvcrt.getch() == b'o' or v.vb is None):
        #     print("Nokov Paused...")
        #     while msvcrt.getch() != b'r':
        #         time.sleep(0.5)
        #         msvcrt.getch()
        #     print("Nokov Resumed")
        try:
            for i in range(7):
                gp[i]=np.array([p[i].x, p[i].y, p[i].phi], dtype='float16')
                gv[i]=np.array([v[i].x, v[i].y], dtype='float16')
                
        except Exception as e:
            break

        # plt.cla()
        # plt.subplot(411)
        # # print([get_len(vl[i][1][0],vl[i][1][1])  for i in range(vl.shape[0])])
        # plt.plot([get_len(vl[i][0][0],vl[i][0][1])  for i in range(vl.shape[0])])
        # plt.subplot(412)
        # plt.plot([get_len(vl[i][1][0],vl[i][1][1]) for i in range(vl.shape[0])])
        # plt.subplot(413)
        # plt.plot([get_len(vl[i][2][0],vl[i][2][1]) for i in range(vl.shape[0])])
        # plt.subplot(414)
        # plt.plot([get_len(vl[i][3][0],vl[i][3][1]) for i in range(vl.shape[0])])
        # plt.pause(1)
        # time.sleep(1)
        # if msvcrt.kbhit() and msvcrt.getch() == b't' or falt:
        #     break
    
    done=True
    event.clear()
    # plt.savefig('./test.png')
    # plt.ioff()
    # plt.show()
    
     
    exit(0)   

# if __name__=='__main__':
#     argv=sys.argv[1:]
#     print(argv)
#     eval(argv[0])()